//
// Created by 29245 on 2025/8/28.
//

#include "action.h"
#include "handle.h"
#include "delay.h"
#include <stdio.h>
#include <math.h>
#include "spider_leg_kinematics.h"
#include <stdlib.h>
#include "oled.h"
#include "PS2.h"

// =============================================================================
// 宏定义和常量
// =============================================================================

// 控制模式状态机变量
static control_mode_t control_mode = CONTROL_MODE_IDLE;

// 摇杆控制状态变量
static joystick_state_t joystick_state = JOYSTICK_STATE_IDLE;
static int last_ps2_lx = 0;
static int last_ps2_ly = 0;
extern PS2_TypeDef ps2;
// 旋转控制状态变量
static rotation_state_t rotation_state = ROTATION_STATE_IDLE;
static int last_ps2_rx = 0;
static uint32_t rotation_start_time = 0;
static uint32_t rotation_duration = 500; // 默认旋转持续时间500ms

// 步态参数
#define STRIDE_LENGTH 60.0f           // 步幅长度
#define LIFT_HEIGHT 20.0f             // 抬腿高度
#define STEP_DURATION_MS 720          // 每步持续时间（毫秒）
#define STEP_CYCLE_COUNT 1            // 步态周期数

// 旋转参数
#define ROTATION_ANGLE 20.0f          // 旋转角度（度）
#define ROTATION_LIFT_HEIGHT 20.0f    // 旋转时抬腿高度
#define ROTATION_STEP_DURATION_MS 720 // 旋转步态持续时间

// 四足步态参数
#define LEG_COUNT 6                   // 腿的数量
#define TROT_STEP_DURATION_MS STEP_DURATION_MS  // 每步持续时间

// 定点数运算相关定义 (使用16.16格式)
typedef int32_t fixed_t;
#define FIXED_SHIFT 16
#define FIXED_ONE (1 << FIXED_SHIFT)
#define FLOAT_TO_FIXED(f) ((fixed_t)((f) * FIXED_ONE))
#define FIXED_TO_FLOAT(f) ((float)(f) / FIXED_ONE)
#define FIXED_MUL(a, b) (((int64_t)(a) * (b)) >> FIXED_SHIFT)
#define FIXED_DIV(a, b) (((int64_t)(a) << FIXED_SHIFT) / (b))

// 预计算的cos和sin查找表，用于提高三角函数计算效率
#define TRIG_TABLE_SIZE 256
static float cos_table[TRIG_TABLE_SIZE];
static float sin_table[TRIG_TABLE_SIZE];
static uint8_t trig_table_initialized = 0;

// 预计算的S曲线查找表
#define S_CURVE_TABLE_SIZE 256
static float s_curve_table[S_CURVE_TABLE_SIZE];
static uint8_t s_curve_table_initialized = 0;

// =============================================================================
// 数据结构定义
// =============================================================================

// 步态状态机定义
typedef enum {
    GAIT_STATE_IDLE,           // 空闲状态
    GAIT_STATE_ACTIVE,         // 活动状态（正在执行步态）
    GAIT_STATE_FINISHING,      // 结束状态（完成最后一个周期）
} GaitState;

// 步态控制结构体
static struct {
    GaitState state;           // 当前状态
    int cycle_count;           // 已完成周期数
    int half_cycle_count;      // 已完成半周期数
    uint32_t half_cycle_start_time; // 半周期开始时间
    int is_swing_group_a;      // 当前摆动组是否为A组
    // 新增成员：用于连续移动控制
    float target_dir_x;        // 目标方向X分量
    float target_dir_y;        // 目标方向Y分量
    uint32_t move_duration;    // 移动持续时间（毫秒）
    uint32_t start_time;       // 移动开始时间
} gait_ctrl = {GAIT_STATE_IDLE, 0, 0, 0, 1};

// 旋转控制结构体
static struct {
    GaitState state;           // 当前状态
    int cycle_count;           // 已完成周期数
    int half_cycle_count;      // 已完成半周期数
    uint32_t half_cycle_start_time; // 半周期开始时间
    int is_swing_group_a;      // 当前摆动组是否为A组
    uint32_t move_duration;    // 移动持续时间（毫秒）
    uint32_t start_time;       // 移动开始时间
    int direction;             // 旋转方向：1表示逆时针，-1表示顺时针
} turn_ctrl = {GAIT_STATE_IDLE, 0, 0, 0, 1, 0, 0, 0};

// =============================================================================
// 内部函数声明
// =============================================================================

// 查找表相关函数
static void init_trig_tables(void);
static void init_s_curve_table(void);
static float fast_cos(float angle);
static float fast_sin(float angle);
static float fast_s_curve(float t);
static float fast_rsqrt(float number);

// 批量控制函数
static void batch_leg_move_to_global_positions_opt(const float positions[6][3]);

// =============================================================================
// 控制模式相关函数
// =============================================================================

/**
 * 设置控制模式
 */
void action_set_control_mode(control_mode_t mode) {
    control_mode = mode;
}

/**
 * 获取当前控制模式
 */
control_mode_t action_get_control_mode(void) {
    return control_mode;
}

/**
 * 更新控制模式状态机
 * @param ps2_red_light: PS2红灯状态 (1表示连接正常)
 * @param ps2_lx: 左摇杆X轴值
 * @param ps2_ly: 左摇杆Y轴值
 */
void action_update_control_mode(uint8_t ps2_mode, int ps2_lx, int ps2_ly) {
    switch (control_mode) {
        case CONTROL_MODE_IDLE:
            if (ps2.mode == 65) {

                control_mode = CONTROL_MODE_BUTTON;  // 进入按键控制模式
            } else if ((ps2_lx > 10 || ps2_lx < -10) || (ps2_ly > 10 || ps2_ly < -10)) {

                control_mode = CONTROL_MODE_JOYSTICK;  // 进入摇杆控制模式
                joystick_state = JOYSTICK_STATE_IDLE;  // 重置摇杆状态
                rotation_state = ROTATION_STATE_IDLE;  // 重置旋转状态
            }
            break;

        case CONTROL_MODE_BUTTON:

            if (ps2.mode == 115) {
                OLED_NewFrame();
                // OLED_DrawImage(30, -20, &终极蜘蛛侠Img, OLED_COLOR_REVERSED);
                OLED_PrintString(15, 32, "摇杆模式已开启", &font15x15, OLED_COLOR_NORMAL);
                OLED_PrintString(15, 50, "按下MODE切换模式", &font15x15, OLED_COLOR_NORMAL);
                OLED_ShowFrame();

                control_mode = CONTROL_MODE_IDLE;  // 退出按键控制模式
                action_stop_continuous();
                action_turn_stop();
            }
            break;

        case CONTROL_MODE_JOYSTICK:

            if (ps2.mode == 65) {

                OLED_NewFrame();
                // OLED_DrawImage(30, -20, &终极蜘蛛侠Img, OLED_COLOR_REVERSED);
                OLED_PrintString(15, 32, "按键模式已开启", &font15x15, OLED_COLOR_NORMAL);
                OLED_PrintString(15, 50, "按下MODE切换模式", &font15x15, OLED_COLOR_NORMAL);
                OLED_ShowFrame();

                control_mode = CONTROL_MODE_BUTTON;  // 切换到按键控制模式
                action_stop_continuous();
                action_turn_stop();
                joystick_state = JOYSTICK_STATE_IDLE;  // 重置摇杆状态
                rotation_state = ROTATION_STATE_IDLE;  // 重置旋转状态
            }
            break;

    }
}

// =============================================================================
// 摇杆控制相关函数
// =============================================================================

/**
 * 根据摇杆值控制机器人移动和旋转
 * @param ps2_lx: 左摇杆X轴值 (-127到127)
 * @param ps2_ly: 左摇杆Y轴值 (-127到127)
 */
void action_move_by_joystick(int ps2_lx, int ps2_ly) {
    // 检查左摇杆是否在死区外
    if ((ps2_lx > 10 || ps2_lx < -10) || (ps2_ly > 10 || ps2_ly < -10)) {
        // 检查摇杆值是否发生显著变化
        if (abs(ps2_lx - last_ps2_lx) > 10 || abs(ps2_ly - last_ps2_ly) > 10) {
            // 摇杆位置发生显著变化，立即更新移动方向
            // 更新最后的摇杆值
            last_ps2_lx = ps2_lx;
            last_ps2_ly = ps2_ly;

            // 根据LX和LY计算方向向量
            float joystick_angle_left = atan2f((float)ps2_ly, (float)ps2_lx);
            // 将弧度转换为角度
            joystick_angle_left = joystick_angle_left * 180.0f / M_PI;
            // 转换为0-360度范围
            if (joystick_angle_left < 0) {
                joystick_angle_left += 360.0f;
            }

            // 将角度转换为方向向量
            float angle_rad = joystick_angle_left * M_PI / 180.0f;
            float target_x = cosf(angle_rad);  // 前后移动
            float target_y = sinf(angle_rad);  // 左右移动

            // 立即停止当前动作
            action_stop_continuous();

            // 调用通用移动函数，开始新方向的移动
            action_move_to_direction(target_x, target_y, 0);
            // 更新状态
            joystick_state = JOYSTICK_STATE_MOVING;
        } else if (joystick_state == JOYSTICK_STATE_IDLE ||
                  (joystick_state == JOYSTICK_STATE_MOVING && gait_ctrl.state == GAIT_STATE_IDLE)) {
            // 摇杆位置没有显著变化，但需要确保机器人继续移动
            // 根据当前摇杆值计算方向向量
            float joystick_angle_left = atan2f((float)ps2_ly, (float)ps2_lx);
            joystick_angle_left = joystick_angle_left * 180.0f / M_PI;
            if (joystick_angle_left < 0) {
                joystick_angle_left += 360.0f;
            }

            float angle_rad = joystick_angle_left * M_PI / 180.0f;
            float target_x = cosf(angle_rad);
            float target_y = sinf(angle_rad);

            // 调用通用移动函数
            action_move_to_direction(target_x, target_y, 0);
            joystick_state = JOYSTICK_STATE_MOVING;
        }
    } else if (joystick_state != JOYSTICK_STATE_IDLE) {
        // 左摇杆回到中心，立即停止移动
        action_stop_continuous();
    }
}

// =============================================================================
// 基本动作相关函数
// =============================================================================

/**
 * 站立动作函数
 * 使用全局坐标系控制所有腿到标准站立位置
 */
void action_stand(void) {
    //printf("执行站立动作\n");

    // 初始化查找表
    init_trig_tables();
    init_s_curve_table();

    // 控制6条腿到标准站立位置
    // 腿1: 前方左侧
    leg_move_to_global_position(1, P1X, P1Y, STAND_DEFAULT_HEIGHT);

    // 腿2: 中间左侧
    leg_move_to_global_position(2, P2X, P2Y, STAND_DEFAULT_HEIGHT);

    // 腿3: 后方左侧
    leg_move_to_global_position(3, P3X, P3Y, STAND_DEFAULT_HEIGHT);

    // 腿4: 后方右侧
    leg_move_to_global_position(4, P4X, P4Y, STAND_DEFAULT_HEIGHT);

    // 腿5: 中间右侧
    leg_move_to_global_position(5, P5X, P5Y, STAND_DEFAULT_HEIGHT);

    // 腿6: 前方右侧
    leg_move_to_global_position(6, P6X, P6Y, STAND_DEFAULT_HEIGHT);

    //printf("站立动作完成\n");
}

// =============================================================================
// 移动控制相关函数
// =============================================================================

/**
 * 指定高度站立动作函数
 * 使用全局坐标系控制所有腿到指定高度的站立位置
 * @param height: 站立高度值
 */
void action_stand_at_height(float height) {
    //printf("执行指定高度站立动作: %.2f\n", height);

    // 初始化查找表
    init_trig_tables();
    init_s_curve_table();

    // 控制6条腿到指定高度的站立位置
    // 腿1: 前方左侧
    leg_move_to_global_position(1, P1X, P1Y, height);

    // 腿2: 中间左侧
    leg_move_to_global_position(2, P2X, P2Y, height);

    // 腿3: 后方左侧
    leg_move_to_global_position(3, P3X, P3Y, height);

    // 腿4: 后方右侧
    leg_move_to_global_position(4, P4X, P4Y, height);

    // 腿5: 中间右侧
    leg_move_to_global_position(5, P5X, P5Y, height);

    // 腿6: 前方右侧
    leg_move_to_global_position(6, P6X, P6Y, height);

    //printf("指定高度站立动作完成\n");
}


/**
 * 通用移动函数 - 通过目标方向向量实现任意方向移动
 *
 * @param target_x: 目标X方向分量 (正数表示向前)
 * @param target_y: 目标Y方向分量 (正数表示向左)
 * @param duration_ms: 移动持续时间（毫秒），0表示无限持续
 */
void action_move_to_direction(float target_x, float target_y, uint32_t duration_ms) {
    // 归一化方向向量
    float magnitude = sqrtf(target_x * target_x + target_y * target_y);

    if (magnitude < 0.001f) {
        // 如果方向为零，直接返回
        return;
    }

    // 计算单位方向向量
    float dir_x = target_x / magnitude;
    float dir_y = target_y / magnitude;

    // 设置为活动状态
    gait_ctrl.state = GAIT_STATE_ACTIVE;
    gait_ctrl.cycle_count = 0;
    gait_ctrl.half_cycle_count = 0;
    gait_ctrl.is_swing_group_a = 1;
    gait_ctrl.half_cycle_start_time = HAL_GetTick();

    // 存储目标方向和持续时间
    gait_ctrl.target_dir_x = dir_x;
    gait_ctrl.target_dir_y = dir_y;
    gait_ctrl.move_duration = duration_ms;
    gait_ctrl.start_time = HAL_GetTick();
}

/**
 * 通用移动函数 - 通过角度实现任意方向移动
 *
 * @param angle_degrees: 移动角度（度），0°表示向前，90°表示向左，180°表示向后，270°表示向右
 * @param duration_ms: 移动持续时间（毫秒），0表示无限持续
 */
void action_move_by_angle(float angle_degrees, uint32_t duration_ms) {
    // 将角度转换为弧度
    float angle_rad = angle_degrees * M_PI / 180.0f;

    // 计算方向分量
    float dir_x = cosf(angle_rad);  // X方向分量
    float dir_y = sinf(angle_rad);  // Y方向分量

    // 调用通用移动函数
    action_move_to_direction(dir_x, dir_y, duration_ms);
}

/**
 * 停止连续移动
 */
void action_stop_continuous(void) {
    gait_ctrl.state = GAIT_STATE_IDLE;
    gait_ctrl.cycle_count = 0;
    gait_ctrl.half_cycle_count = 0;
    gait_ctrl.is_swing_group_a = 1;
    gait_ctrl.half_cycle_start_time = 0;
    gait_ctrl.move_duration = 0;
    gait_ctrl.start_time = 0;

    // 重置摇杆状态
    joystick_state = JOYSTICK_STATE_IDLE;
}

/**
 * 检查是否在连续移动
 */
uint8_t is_action_moving_continuous(void) {
    return (gait_ctrl.state == GAIT_STATE_ACTIVE);
}

// =============================================================================
// 旋转控制相关函数
// =============================================================================

/**
 * 启动逆时针旋转
 */
void action_turn_left_start(void) {
    turn_ctrl.state = GAIT_STATE_ACTIVE;
    turn_ctrl.cycle_count = 0;
    turn_ctrl.half_cycle_count = 0;
    turn_ctrl.is_swing_group_a = 1;
    turn_ctrl.half_cycle_start_time = HAL_GetTick();
    turn_ctrl.move_duration = 0;  // 无限持续
    turn_ctrl.start_time = HAL_GetTick();
    turn_ctrl.direction = 1;  // 逆时针
}

/**
 * 启动顺时针旋转
 */
void action_turn_right_start(void) {
    turn_ctrl.state = GAIT_STATE_ACTIVE;
    turn_ctrl.cycle_count = 0;
    turn_ctrl.half_cycle_count = 0;
    turn_ctrl.is_swing_group_a = 1;
    turn_ctrl.half_cycle_start_time = HAL_GetTick();
    turn_ctrl.move_duration = 0;  // 无限持续
    turn_ctrl.start_time = HAL_GetTick();
    turn_ctrl.direction = -1;  // 顺时针
}

/**
 * 停止旋转
 */
void action_turn_stop(void) {
    turn_ctrl.state = GAIT_STATE_IDLE;
    turn_ctrl.cycle_count = 0;
    turn_ctrl.half_cycle_count = 0;
    turn_ctrl.is_swing_group_a = 1;
    turn_ctrl.half_cycle_start_time = 0;
    turn_ctrl.move_duration = 0;
    turn_ctrl.start_time = 0;
    turn_ctrl.direction = 0;

    // 重置旋转状态
    rotation_state = ROTATION_STATE_IDLE;
}

/**
 * 检查是否正在旋转
 */
uint8_t is_action_turn_running(void) {
    return (turn_ctrl.state == GAIT_STATE_ACTIVE);
}

// =============================================================================
// 执行函数
// =============================================================================

/**
 * 通用移动函数 - 实际执行移动逻辑
 * 这个函数需要在主循环中定期调用
 */
void action_move_execute(void) {
    static uint32_t last_update_time = 0;
    uint32_t current_time = HAL_GetTick();

    // 限制更新频率，避免过于频繁的计算，但保持在合理范围内以保证流畅性
    if (current_time - last_update_time < 4) {  // 约250Hz
        return;
    }
    last_update_time = current_time;

    // 准备批量控制数组
    static float target_positions[6][3] = {
        {P1X, P1Y, STAND_DEFAULT_HEIGHT},
        {P2X, P2Y, STAND_DEFAULT_HEIGHT},
        {P3X, P3Y, STAND_DEFAULT_HEIGHT},
        {P4X, P4Y, STAND_DEFAULT_HEIGHT},
        {P5X, P5Y, STAND_DEFAULT_HEIGHT},
        {P6X, P6Y, STAND_DEFAULT_HEIGHT}
    };

    uint8_t need_update = 0;

    // 执行普通移动
    if (gait_ctrl.state == GAIT_STATE_ACTIVE) {
        if (gait_ctrl.move_duration > 0 &&
            (current_time - gait_ctrl.start_time) >= gait_ctrl.move_duration) {
            action_stop_continuous();
            return;
        }

        // 半周期检查
        if ((current_time - gait_ctrl.half_cycle_start_time) >= (STEP_DURATION_MS / 2)) {
            gait_ctrl.half_cycle_count++;
            gait_ctrl.is_swing_group_a = !gait_ctrl.is_swing_group_a;

            if (gait_ctrl.half_cycle_count % 2 == 0) {
                gait_ctrl.cycle_count++;
            }
            gait_ctrl.half_cycle_start_time = current_time;
        }

        float progress = (float)(current_time - gait_ctrl.half_cycle_start_time) / (STEP_DURATION_MS / 2);
        if (progress > 1.0f) progress = 1.0f;

        // 使用查找表快速计算S曲线插值来改善流畅度
        float smooth_progress = fast_s_curve(progress);

        // 垂直方向使用S曲线插值确保起点和终点的速度为0
        float lift_factor = fast_s_curve(progress);

        // 四足步态分组（模拟四足动物小跑步态）
        // 将6条腿分为两组，模仿四足步态
        // 组A: 腿1(左前) + 腿3(左后) + 腿5(右中)
        static const int group_a[3] = {0, 2, 4};
        // 组B: 腿2(左中) + 腿4(右后) + 腿6(右前)
        static const int group_b[3] = {1, 3, 5};

        const int *swing_group = gait_ctrl.is_swing_group_a ? group_a : group_b;
        const int *support_group = gait_ctrl.is_swing_group_a ? group_b : group_a;

        // 定义起始位置数组（避免重复判断）
        static const float start_positions[6][3] = {
                {P1X, P1Y, STAND_DEFAULT_HEIGHT},
                {P2X, P2Y, STAND_DEFAULT_HEIGHT},
                {P3X, P3Y, STAND_DEFAULT_HEIGHT},
                {P4X, P4Y, STAND_DEFAULT_HEIGHT},
                {P5X, P5Y, STAND_DEFAULT_HEIGHT},
                {P6X, P6Y, STAND_DEFAULT_HEIGHT}
        };

        // 预计算公共变量以减少重复计算
        const float dx_factor = STRIDE_LENGTH * 0.5f * (smooth_progress - 0.5f);
        const float dx_swing = gait_ctrl.target_dir_x * dx_factor;
        const float dy_swing = gait_ctrl.target_dir_y * dx_factor;
        const float dx_support = -dx_swing;
        const float dy_support = -dy_swing;
        const float lift_height = LIFT_HEIGHT * lift_factor;

        // 摆动组：沿目标方向移动 (使用S曲线插值改善流畅度)
        for (int i = 0; i < 3; i++) {
            const int leg = swing_group[i];
            target_positions[leg][0] = start_positions[leg][0] + dx_swing;
            target_positions[leg][1] = start_positions[leg][1] + dy_swing;
            target_positions[leg][2] = start_positions[leg][2] + lift_height;
        }

        // 支撑组：沿反方向移动（保持身体稳定）
        for (int i = 0; i < 3; i++) {
            const int leg = support_group[i];
            target_positions[leg][0] = start_positions[leg][0] + dx_support;
            target_positions[leg][1] = start_positions[leg][1] + dy_support;
            target_positions[leg][2] = start_positions[leg][2];  // 不抬腿
        }

        need_update = 1;
    }

    // 执行旋转动作
    if (turn_ctrl.state == GAIT_STATE_ACTIVE) {
        // 半周期检查
        if ((current_time - turn_ctrl.half_cycle_start_time) >= (ROTATION_STEP_DURATION_MS / 2)) {
            turn_ctrl.half_cycle_count++;
            turn_ctrl.is_swing_group_a = !turn_ctrl.is_swing_group_a;

            if (turn_ctrl.half_cycle_count % 2 == 0) {
                turn_ctrl.cycle_count++;
            }
            turn_ctrl.half_cycle_start_time = current_time;
        }

        float progress = (float)(current_time - turn_ctrl.half_cycle_start_time) / (ROTATION_STEP_DURATION_MS / 2);
        if (progress > 1.0f) progress = 1.0f;

        // 使用查找表快速计算S曲线插值来改善流畅度
        float smooth_progress = fast_s_curve(progress);

        // 垂直方向使用S曲线插值确保起点和终点的速度为0
        float lift_factor = fast_s_curve(progress);

        // 四足步态旋转分组
        static const int group_a[3] = {0, 2, 4};  // 腿1, 3, 5
        static const int group_b[3] = {1, 3, 5};  // 腿2, 4, 6

        const int *swing_group = turn_ctrl.is_swing_group_a ? group_a : group_b;
        const int *support_group = turn_ctrl.is_swing_group_a ? group_b : group_a;

        // 定义起始位置数组（避免重复判断）
        static const float start_positions[6][3] = {
                {P1X, P1Y, STAND_DEFAULT_HEIGHT},
                {P2X, P2Y, STAND_DEFAULT_HEIGHT},
                {P3X, P3Y, STAND_DEFAULT_HEIGHT},
                {P4X, P4Y, STAND_DEFAULT_HEIGHT},
                {P5X, P5Y, STAND_DEFAULT_HEIGHT},
                {P6X, P6Y, STAND_DEFAULT_HEIGHT}
        };

        // 旋转动作
        const float rotation_direction = (float)turn_ctrl.direction; // 1为逆时针，-1为顺时针

        // 预计算公共变量
        const float angle_offset = rotation_direction * ROTATION_ANGLE * M_PI / 180.0f * (smooth_progress - 0.5f);
        const float lift_height = ROTATION_LIFT_HEIGHT * lift_factor;

        // 摆动组：沿圆弧移动实现旋转 (使用S曲线插值改善流畅度)
        for (int i = 0; i < 3; i++) {
            const int leg = swing_group[i];
            // 使用预计算的值避免重复计算
            const float x = start_positions[leg][0];
            const float y = start_positions[leg][1];
            const float radius_sq = x * x + y * y;  // 半径的平方
            const float radius = 1.0f / fast_rsqrt(radius_sq); // 使用快速平方根倒数算法

            // 计算旋转角度（转换为弧度）
            const float current_angle = atan2f(y, x); // atan2f相对较快且精度要求高
            const float target_angle = current_angle + angle_offset;

            const float cos_target = fast_cos(target_angle);
            const float sin_target = fast_sin(target_angle);
            target_positions[leg][0] = radius * cos_target;
            target_positions[leg][1] = radius * sin_target;
            target_positions[leg][2] = start_positions[leg][2] + lift_height;
        }

        // 支撑组：保持原位
        for (int i = 0; i < 3; i++) {
            const int leg = support_group[i];
            target_positions[leg][0] = start_positions[leg][0];
            target_positions[leg][1] = start_positions[leg][1];
            target_positions[leg][2] = start_positions[leg][2];  // 不抬腿
        }

        need_update = 1;
    }

    // 如果需要更新位置，则批量控制所有腿部
    if (need_update) {
        batch_leg_move_to_global_positions_opt(target_positions);
    }

    // 处理摇杆控制状态转换
    if (control_mode == CONTROL_MODE_JOYSTICK) {
        if (joystick_state == JOYSTICK_STATE_STOPPING) {
            // 一旦进入STOPPING状态，立即停止移动
            action_stop_continuous();
            joystick_state = JOYSTICK_STATE_IDLE;
        }

        if (rotation_state == ROTATION_STATE_ROTATING &&
            (current_time - rotation_start_time) >= rotation_duration) {
            action_turn_stop();
            rotation_state = ROTATION_STATE_IDLE;
        }
    }
}

/**
 * 旋转动作执行函数 - 实际执行旋转逻辑
 * 这个函数需要在主循环中定期调用
 */
void action_turn_execute(void) {
    static uint32_t last_update_time = 0;
    uint32_t current_time = HAL_GetTick();

    // 限制更新频率，避免过于频繁的计算
    if (current_time - last_update_time < 4) {  // 约250Hz
        return;
    }
    last_update_time = current_time;

    if (turn_ctrl.state != GAIT_STATE_ACTIVE) {
        return;
    }

    // 准备批量控制数组
    static float target_positions[6][3] = {
        {P1X, P1Y, STAND_DEFAULT_HEIGHT},
        {P2X, P2Y, STAND_DEFAULT_HEIGHT},
        {P3X, P3Y, STAND_DEFAULT_HEIGHT},
        {P4X, P4Y, STAND_DEFAULT_HEIGHT},
        {P5X, P5Y, STAND_DEFAULT_HEIGHT},
        {P6X, P6Y, STAND_DEFAULT_HEIGHT}
    };

    // 半周期检查
    if ((current_time - turn_ctrl.half_cycle_start_time) >= (ROTATION_STEP_DURATION_MS / 2)) {
        turn_ctrl.half_cycle_count++;
        turn_ctrl.is_swing_group_a = !turn_ctrl.is_swing_group_a;

        if (turn_ctrl.half_cycle_count % 2 == 0) {
            turn_ctrl.cycle_count++;
        }
        turn_ctrl.half_cycle_start_time = current_time;
    }

    float progress = (float)(current_time - turn_ctrl.half_cycle_start_time) / (ROTATION_STEP_DURATION_MS / 2);
    if (progress > 1.0f) progress = 1.0f;

    // 使用查找表快速计算S曲线插值来改善流畅度
    float smooth_progress = fast_s_curve(progress);

    // 垂直方向使用S曲线插值确保起点和终点的速度为0
    float lift_factor = fast_s_curve(progress);

    static const int group_a[3] = {0, 2, 4};  // 腿1, 3, 5
    static const int group_b[3] = {1, 3, 5};  // 腿2, 4, 6

    const int *swing_group = turn_ctrl.is_swing_group_a ? group_a : group_b;
    const int *support_group = turn_ctrl.is_swing_group_a ? group_b : group_a;

    // 定义起始位置数组（避免重复判断）
    static const float start_positions[6][3] = {
            {P1X, P1Y, STAND_DEFAULT_HEIGHT},
            {P2X, P2Y, STAND_DEFAULT_HEIGHT},
            {P3X, P3Y, STAND_DEFAULT_HEIGHT},
            {P4X, P4Y, STAND_DEFAULT_HEIGHT},
            {P5X, P5Y, STAND_DEFAULT_HEIGHT},
            {P6X, P6Y, STAND_DEFAULT_HEIGHT}
    };

    // 旋转动作
    const float rotation_direction = (float)turn_ctrl.direction; // 1为逆时针，-1为顺时针

    // 预计算公共变量
    const float angle_offset = rotation_direction * ROTATION_ANGLE * M_PI / 180.0f * (smooth_progress - 0.5f);
    const float lift_height = ROTATION_LIFT_HEIGHT * lift_factor;

    // 摆动组：沿圆弧移动实现旋转
    for (int i = 0; i < 3; i++) {
        const int leg = swing_group[i];
        // 使用预计算的值避免重复计算
        const float x = start_positions[leg][0];
        const float y = start_positions[leg][1];
        const float radius_sq = x * x + y * y;  // 半径的平方
        const float radius = 1.0f / fast_rsqrt(radius_sq); // 使用快速平方根倒数算法

        // 计算旋转角度（转换为弧度）
        const float current_angle = atan2f(y, x); // atan2f相对较快且精度要求高
        const float target_angle = current_angle + angle_offset;

        const float cos_target = fast_cos(target_angle);
        const float sin_target = fast_sin(target_angle);
        target_positions[leg][0] = radius * cos_target;
        target_positions[leg][1] = radius * sin_target;
        target_positions[leg][2] = start_positions[leg][2] + lift_height;
    }

    // 支撑组：保持原位
    for (int i = 0; i < 3; i++) {
        const int leg = support_group[i];
        target_positions[leg][0] = start_positions[leg][0];
        target_positions[leg][1] = start_positions[leg][1];
        target_positions[leg][2] = start_positions[leg][2];  // 不抬腿
    }

    // 批量控制所有腿部
    batch_leg_move_to_global_positions_opt(target_positions);
}

// =============================================================================
// 内部函数实现
// =============================================================================

/**
 * 优化批量腿部位置控制函数，减少重复计算
 */
static void batch_leg_move_to_global_positions_opt(const float positions[6][3]) {
    // 预先声明所有需要的变量，减少内存分配
    static ServoAngles servo_angles[6];
    static LegStatus statuses[6];
    static D4Coord global_coords[6];

    // 批量准备坐标数据
    for (int i = 0; i < 6; i++) {
        global_coords[i].x = positions[i][0];
        global_coords[i].y = positions[i][1];
        global_coords[i].z = positions[i][2];
    }

    // 批量计算所有腿部的逆解
    uint8_t all_success = 1;
    for (int i = 0; i < 6; i++) {
        statuses[i] = Leg_InverseKinematicsGlobal(i, global_coords[i], &servo_angles[i]);
        if (statuses[i] != LEG_OK) {
            all_success = 0;
        }
    }

    // 批量控制舵机
    if (all_success) {
        // 如果所有腿部都能到达目标位置，则批量控制舵机
        for (int i = 0; i < 6; i++) {
            leg_handle(i + 1,
                      (int16_t)servo_angles[i].body,
                      (int16_t)servo_angles[i].limb,
                      (int16_t)servo_angles[i].foot);
        }
    } else {
        // 如果有腿部无法到达目标位置，则逐个处理
        for (int i = 0; i < 6; i++) {
            if (statuses[i] == LEG_OK) {
                leg_handle(i + 1,
                          (int16_t)servo_angles[i].body,
                          (int16_t)servo_angles[i].limb,
                          (int16_t)servo_angles[i].foot);
            }
        }
    }
}

/**
 * 初始化三角函数查找表
 */
static void init_trig_tables(void) {
    if (trig_table_initialized) return;

    for (int i = 0; i < TRIG_TABLE_SIZE; i++) {
        float angle = (2.0f * M_PI * i) / TRIG_TABLE_SIZE;
        cos_table[i] = cosf(angle);
        sin_table[i] = sinf(angle);
    }
    trig_table_initialized = 1;
}

/**
 * 初始化S曲线查找表
 */
static void init_s_curve_table(void) {
    if (s_curve_table_initialized) return;

    for (int i = 0; i < S_CURVE_TABLE_SIZE; i++) {
        float t = (float)i / (S_CURVE_TABLE_SIZE - 1);
        s_curve_table[i] = 0.5f * (1.0f - cosf(M_PI * t));
        // s_curve_table[i] = t * t * t * (t * (t * 6 - 15) + 10);

    }
    s_curve_table_initialized = 1;
}

/**
 * 使用查找表快速计算cos值 (-π to π)
 */
static float fast_cos(float angle) {
    // 将角度归一化到0-2π范围
    while (angle < 0) angle += 2.0f * M_PI;
    while (angle >= 2.0f * M_PI) angle -= 2.0f * M_PI;

    // 映射到查找表索引
    int index = (int)((angle * TRIG_TABLE_SIZE) / (2.0f * M_PI));
    if (index >= TRIG_TABLE_SIZE) index = TRIG_TABLE_SIZE - 1;

    return cos_table[index];
}

/**
 * 使用查找表快速计算sin值 (-π to π)
 */
static float fast_sin(float angle) {
    // 将角度归一化到0-2π范围
    while (angle < 0) angle += 2.0f * M_PI;
    while (angle >= 2.0f * M_PI) angle -= 2.0f * M_PI;

    // 映射到查找表索引
    int index = (int)((angle * TRIG_TABLE_SIZE) / (2.0f * M_PI));
    if (index >= TRIG_TABLE_SIZE) index = TRIG_TABLE_SIZE - 1;

    return sin_table[index];
}

/**
 * 使用查找表快速计算S曲线值 (0 to 1)
 */
static float fast_s_curve(float t) {
    if (t <= 0.0f) return 0.0f;
    if (t >= 1.0f) return 1.0f;

    int index = (int)(t * (S_CURVE_TABLE_SIZE - 1));
    return s_curve_table[index];
}

/**
 * 快速平方根倒数算法 (类似雷神之锤III算法)
 */
static float fast_rsqrt(float number) {
    long i;
    float x2, y;
    const float threehalfs = 1.5F;

    x2 = number * 0.5F;
    y  = number;
    i  = * (long *) &y;          // 将浮点数位模式解释为长整型
    i  = 0x5f3759df - (i >> 1);  // 魔法数字和位操作
    y  = * (float *) &i;
    y  = y * (threehalfs - (x2 * y * y)); // 牛顿法迭代一次
    return y;
}


// =============================================================================
// 观赏动作
// =============================================================================
// ... existing code ...

/**
 * 身体摆动动作函数 - 保持脚尖位置不变，身体中心按一定轨迹摆动
 * @param direction: 摆动方向 (0=前后摆动, 1=左右摆动)
 * @param amplitude: 摆动幅度
 * @param speed: 摆动速度
 */
void action_body_sway(int direction, float amplitude, int speed) {
    //printf("执行身体摆动动作\r\n");

    // 初始化查找表
    init_trig_tables();
    init_s_curve_table();

    // 摆动周期参数
    const int total_steps = 40;  // 一个完整周期的步数
    const float height = STAND_DEFAULT_HEIGHT;

    // 记录初始脚尖位置
    float initial_foot_x[6] = {P1X, P2X, P3X, P4X, P5X, P6X};
    float initial_foot_y[6] = {P1Y, P2Y, P3Y, P4Y, P5Y, P6Y};

    // 计算身体中心点
    float center_x = 0, center_y = 0;
    for (int i = 0; i < 6; i++) {
        center_x += initial_foot_x[i];
        center_y += initial_foot_y[i];
    }
    center_x /= 6.0f;
    center_y /= 6.0f;

    // 执行摆动动作
    for (int step = 0; step < total_steps * 2; step++) {  // 执行两个完整周期
        float phase = (float)step / total_steps * 2 * M_PI;
        float sway_value = amplitude * sinf(phase);

        // 计算新的身体中心位置
        float new_center_x = center_x;
        float new_center_y = center_y;

        if (direction == 0) {
            // 前后摆动
            new_center_x += sway_value;
        } else {
            // 左右摆动
            new_center_y += sway_value;
        }

        // 计算每条腿的新位置（保持脚尖位置相对不变）
        float offset_x = new_center_x - center_x;
        float offset_y = new_center_y - center_y;

        for (int leg = 0; leg < 6; leg++) {
            float target_x = initial_foot_x[leg] + offset_x;
            float target_y = initial_foot_y[leg] + offset_y;
            leg_move_to_global_position(leg + 1, target_x, target_y, height);
        }

        delay_ms(speed);
    }

    // 回到初始站立位置
   // action_stand_at_height(height);
    //printf("身体摆动动作完成\r\n");
}

/**
 * 身体扭动动作函数 - 身体绕中心点旋转扭动
 * @param angle: 最大扭动角度
 * @param speed: 动作速度
 */
void action_body_twist(float angle, int speed) {
   // printf("执行身体扭动动作\r\n");

    // 初始化查找表
    init_trig_tables();
    init_s_curve_table();

    const int total_steps = 40;
    const float height = STAND_DEFAULT_HEIGHT;

    // 记录初始脚尖位置
    float initial_foot_x[6] = {P1X, P2X, P3X, P4X, P5X, P6X};
    float initial_foot_y[6] = {P1Y, P2Y, P3Y, P4Y, P5Y, P6Y};

    // 计算身体中心点
    float center_x = 0, center_y = 0;
    for (int i = 0; i < 6; i++) {
        center_x += initial_foot_x[i];
        center_y += initial_foot_y[i];
    }
    center_x /= 6.0f;
    center_y /= 6.0f;

    // 执行扭动动作
    for (int step = 0; step < total_steps * 2; step++) {
        float phase = (float)step / total_steps * 2 * M_PI;
        float twist_angle = angle * M_PI / 180.0f * sinf(phase);  // 转换为弧度

        float cos_angle = cosf(twist_angle);
        float sin_angle = sinf(twist_angle);

        // 对每条腿计算新的全局坐标
        for (int leg = 0; leg < 6; leg++) {
            // 相对于中心点的位置
            float rel_x = initial_foot_x[leg] - center_x;
            float rel_y = initial_foot_y[leg] - center_y;

            // 应用旋转变换
            float new_x = rel_x * cos_angle - rel_y * sin_angle + center_x;
            float new_y = rel_x * sin_angle + rel_y * cos_angle + center_y;

            leg_move_to_global_position(leg + 1, new_x, new_y, height);
        }

        delay_ms(speed);
    }

    // 回到初始站立位置
    //action_stand_at_height(height);
    //printf("身体扭动动作完成\r\n");
}

/**
 * 身体波浪动作函数 - 身体像波浪一样起伏
 * @param amplitude: 波浪幅度
 * @param speed: 动作速度
 */
void action_body_wave(float amplitude, int speed) {
    //printf("执行身体波浪动作\r\n");

    // 初始化查找表
    init_trig_tables();
    init_s_curve_table();

    const int total_steps = 40;
    const float base_height = STAND_DEFAULT_HEIGHT;

    // 记录初始脚尖位置
    float initial_foot_x[6] = {P1X, P2X, P3X, P4X, P5X, P6X};
    float initial_foot_y[6] = {P1Y, P2Y, P3Y, P4Y, P5Y, P6Y};

    // 执行波浪动作
    for (int step = 0; step < total_steps * 2; step++) {
        float phase = (float)step / total_steps * 2 * M_PI;

        // 对每条腿计算新的全局坐标
        for (int leg = 0; leg < 6; leg++) {
            // 每条腿不同的相位，形成波浪效果
            float leg_phase = phase + (leg * M_PI / 3);  // 每条腿相位差60度
            float height_offset = amplitude * sinf(leg_phase);
            float target_height = base_height + height_offset;

            leg_move_to_global_position(leg + 1, initial_foot_x[leg], initial_foot_y[leg], target_height);
        }

        delay_ms(speed);
    }

    // 回到初始站立位置
    //action_stand_at_height(base_height);
    //printf("身体波浪动作完成\r\n");
}

/**
 * 舞动动作函数 - 综合多种动作的扭动+波浪
 * @param speed: 动作速度
 */
void action_body_combine(int speed) {
    //printf("执行舞动动作\r\n");

    // 4. 组合动作 - 扭动+波浪
    const int total_steps = 40;
    const float base_height = STAND_DEFAULT_HEIGHT;
    const float twist_angle = 15.0f;
    const float wave_amplitude = 15.0f;

    // 记录初始脚尖位置
    float initial_foot_x[6] = {P1X, P2X, P3X, P4X, P5X, P6X};
    float initial_foot_y[6] = {P1Y, P2Y, P3Y, P4Y, P5Y, P6Y};

    // 计算身体中心点
    float center_x = 0, center_y = 0;
    for (int i = 0; i < 6; i++) {
        center_x += initial_foot_x[i];
        center_y += initial_foot_y[i];
    }
    center_x /= 6.0f;
    center_y /= 6.0f;

    // 执行组合动作
    for (int step = 0; step < total_steps * 2; step++) {
        float phase = (float)step / total_steps * 2 * M_PI;

        // 扭动部分
        float twist_angle_rad = twist_angle * M_PI / 180.0f * sinf(phase * 0.5f);
        float cos_twist = cosf(twist_angle_rad);
        float sin_twist = sinf(twist_angle_rad);

        // 波浪部分
        for (int leg = 0; leg < 6; leg++) {
            // 每条腿不同的相位，形成波浪效果
            float leg_phase = phase + (leg * M_PI / 3);  // 每条腿相位差60度
            float height_offset = wave_amplitude * sinf(leg_phase);
            float target_height = base_height + height_offset;

            // 相对于中心点的位置
            float rel_x = initial_foot_x[leg] - center_x;
            float rel_y = initial_foot_y[leg] - center_y;

            // 应用旋转变换
            float new_x = rel_x * cos_twist - rel_y * sin_twist + center_x;
            float new_y = rel_x * sin_twist + rel_y * cos_twist + center_y;

            leg_move_to_global_position(leg + 1, new_x, new_y, target_height);
        }

        delay_ms(speed);
    }

    // 回到初始站立位置
    //action_stand_at_height(base_height);
   // printf("舞蹈表演动作完成\r\n");
}


// ... existing code ...

/**
 * 点头动作函数 - 模拟机器人点头
 * @param count: 点头次数
 * @param speed: 动作速度
 */
void action_nod_head(int count, int speed) {
    //printf("执行点头动作\r\n");

    // 初始化查找表
    init_trig_tables();
    init_s_curve_table();

    const int total_steps = 20;
    const float nod_amplitude = 20.0f; // 点头幅度
    const float base_height = STAND_DEFAULT_HEIGHT;

    // 记录初始脚尖位置
    float initial_foot_x[6] = {P1X, P2X, P3X, P4X, P5X, P6X};
    float initial_foot_y[6] = {P1Y, P2Y, P3Y, P4Y, P5Y, P6Y};

    // 执行点头动作
    for (int i = 0; i < count; i++) {
        for (int step = 0; step < total_steps; step++) {
            float phase = (float)step / total_steps * 2 * M_PI;
            float height_offset = nod_amplitude * sinf(phase);
            float target_height = base_height + height_offset;

            // 前腿降低，后腿抬高（模拟点头）
            for (int leg = 0; leg < 6; leg++) {
                if (leg == 0 || leg == 5) { // 前腿
                    leg_move_to_global_position(leg + 1, initial_foot_x[leg], initial_foot_y[leg], target_height);
                } else if (leg == 2 || leg == 3) { // 后腿
                    leg_move_to_global_position(leg + 1, initial_foot_x[leg], initial_foot_y[leg], base_height - height_offset);
                } else { // 中腿保持不变
                    leg_move_to_global_position(leg + 1, initial_foot_x[leg], initial_foot_y[leg], base_height);
                }
            }

            delay_ms(speed);
        }
    }

    // 回到初始站立位置
   // action_stand_at_height(base_height);
   // printf("点头动作完成\r\n");
}

/**
 * 握手动作函数 - 模拟机器人握手
 * @param speed: 动作速度
 */
void action_handshake(int speed) {
   // printf("执行握手动作\r\n");

    // 初始化查找表
    init_trig_tables();
    init_s_curve_table();

    const int total_steps = 30;
    const float shake_amplitude = 30.0f; // 握手幅度
    const float base_height = STAND_DEFAULT_HEIGHT;

    // 记录初始脚尖位置
    float initial_foot_x[6] = {P1X, P2X, P3X, P4X, P5X, P6X};
    float initial_foot_y[6] = {P1Y, P2Y, P3Y, P4Y, P5Y, P6Y};

    // 先让机器人站立
    action_stand_at_height(base_height);
    delay_ms(500);

    // 抬起右前腿（腿1）
    for (int step = 0; step < total_steps/3; step++) {
        float progress = (float)step / (total_steps/3);
        float target_height = base_height + progress * 40.0f; // 抬高40mm
        leg_move_to_global_position(1, initial_foot_x[0], initial_foot_y[0], target_height);
        delay_ms(speed);
    }

    // 握手动作（上下摆动）
    for (int i = 0; i < 3; i++) { // 握手3次
        for (int step = 0; step < total_steps/3; step++) {
            float phase = (float)step / (total_steps/3) * 2 * M_PI;
            float height_offset = shake_amplitude * sinf(phase);
            float target_height = base_height + 40.0f + height_offset;
            leg_move_to_global_position(1, initial_foot_x[0], initial_foot_y[0], target_height);
            delay_ms(speed);
        }
    }

    // 放下右前腿
    for (int step = 0; step < total_steps/3; step++) {
        float progress = (float)step / (total_steps/3);
        float target_height = base_height + (1.0f - progress) * 40.0f;
        leg_move_to_global_position(1, initial_foot_x[0], initial_foot_y[0], target_height);
        delay_ms(speed);
    }

    // 回到初始站立位置
    //action_stand_at_height(base_height);
   // printf("握手动作完成\r\n");
}

/**
 * 蹲下起立动作函数
 * @param count: 蹲下起立次数
 * @param speed: 动作速度
 */
void action_squat_stand(int count, int speed) {
   // printf("执行蹲下起立动作\r\n");

    // 初始化查找表
    init_trig_tables();
    init_s_curve_table();

    const int total_steps = 30;
    const float squat_height = 50.0f; // 蹲下高度差
    const float base_height = STAND_DEFAULT_HEIGHT;

    // 记录初始脚尖位置
    float initial_foot_x[6] = {P1X, P2X, P3X, P4X, P5X, P6X};
    float initial_foot_y[6] = {P1Y, P2Y, P3Y, P4Y, P5Y, P6Y};

    // 执行蹲下起立动作
    for (int i = 0; i < count; i++) {
        // 蹲下
        for (int step = 0; step < total_steps/2; step++) {
            float progress = (float)step / (total_steps/2);
            float target_height = base_height - progress * squat_height;

            for (int leg = 0; leg < 6; leg++) {
                leg_move_to_global_position(leg + 1, initial_foot_x[leg], initial_foot_y[leg], target_height);
            }

            delay_ms(speed);
        }

        // 起立
        for (int step = 0; step < total_steps/2; step++) {
            float progress = (float)step / (total_steps/2);
            float target_height = (base_height - squat_height) + progress * squat_height;

            for (int leg = 0; leg < 6; leg++) {
                leg_move_to_global_position(leg + 1, initial_foot_x[leg], initial_foot_y[leg], target_height);
            }

            delay_ms(speed);
        }
    }

    // 回到初始站立位置
    //action_stand_at_height(base_height);
    //printf("蹲下起立动作完成\r\n");
}

/**
 * 伸懒腰动作函数
 * @param speed: 动作速度
 */
void action_stretch(int speed) {
    //printf("执行伸懒腰动作\r\n");

    // 初始化查找表
    init_trig_tables();
    init_s_curve_table();

    const int total_steps = 40;
    const float stretch_amplitude = 30.0f; // 伸展幅度
    const float base_height = STAND_DEFAULT_HEIGHT;

    // 记录初始脚尖位置
    float initial_foot_x[6] = {P1X, P2X, P3X, P4X, P5X, P6X};
    float initial_foot_y[6] = {P1Y, P2Y, P3Y, P4Y, P5Y, P6Y};

    // 抬高身体并伸展腿部
    for (int step = 0; step < total_steps/2; step++) {
        float progress = (float)step / (total_steps/2);
        float target_height = base_height + progress * stretch_amplitude;

        // 前后腿交替伸展
        for (int leg = 0; leg < 6; leg++) {
            float x_offset = 0;
            if (leg == 0 || leg == 3) { // 前腿和对角后腿
                x_offset = progress * 20.0f;
            } else if (leg == 5 || leg == 2) { // 前腿和对角后腿
                x_offset = -progress * 20.0f;
            }

            leg_move_to_global_position(leg + 1, initial_foot_x[leg] + x_offset, initial_foot_y[leg], target_height);
        }

        delay_ms(speed);
    }

    // 收回
    for (int step = 0; step < total_steps/2; step++) {
        float progress = (float)step / (total_steps/2);
        float target_height = (base_height + stretch_amplitude) - progress * stretch_amplitude;

        // 前后腿交替收回
        for (int leg = 0; leg < 6; leg++) {
            float x_offset = 0;
            if (leg == 0 || leg == 3) { // 前腿和对角后腿
                x_offset = (1.0f - progress) * 20.0f;
            } else if (leg == 5 || leg == 2) { // 前腿和对角后腿
                x_offset = -(1.0f - progress) * 20.0f;
            }

            leg_move_to_global_position(leg + 1, initial_foot_x[leg] + x_offset, initial_foot_y[leg], target_height);
        }

        delay_ms(speed);
    }

    // 回到初始站立位置
    //action_stand_at_height(base_height);
    //printf("伸懒腰动作完成\r\n");
}

/**
 * 综合表演动作函数
 * @param speed: 动作速度
 */
void action_performance_show(int speed) {
   // printf("执行综合表演动作\r\n");

    // 1. 点头问好
    action_nod_head(2, speed);
    delay_ms(500);

    // 3. 握手
    action_handshake(speed);
    delay_ms(1000);

    // 4. 蹲下起立
    action_squat_stand(2, speed);
    delay_ms(500);

    // 5. 伸懒腰
    action_stretch(speed);
    delay_ms(500);

    // 6. 舞蹈表演

    //printf("综合表演动作完成\r\n");
}

// ... existing code ...

/**
 * 交叉步动作函数 - 模拟人类交叉步行走
 * @param count: 步数
 * @param speed: 动作速度
 */
void action_cross_step(int count, int speed) {
    //printf("执行交叉步动作\r\n");

    const float base_height = STAND_DEFAULT_HEIGHT;
    const float step_height = 20.0f;
    const float step_length = 30.0f;

    // 记录初始脚尖位置
    float initial_foot_x[6] = {P1X, P2X, P3X, P4X, P5X, P6X};
    float initial_foot_y[6] = {P1Y, P2Y, P3Y, P4Y, P5Y, P6Y};

    // 执行交叉步动作
    for (int i = 0; i < count; i++) {
        // 抬起左前腿并向右前方移动
        for (int step = 0; step < 10; step++) {
            float progress = (float)step / 10;
            float target_x = initial_foot_x[0] + progress * step_length;
            float target_y = initial_foot_y[0] - progress * 20.0f;
            float target_z = base_height + progress * step_height;
            leg_move_to_global_position(1, target_x, target_y, target_z);
            delay_ms(speed);
        }

        // 放下左前腿
        for (int step = 0; step < 10; step++) {
            float progress = (float)step / 10;
            float target_x = initial_foot_x[0] + step_length - progress * step_length;
            float target_y = initial_foot_y[0] - 20.0f + progress * 20.0f;
            float target_z = base_height + step_height - progress * step_height;
            leg_move_to_global_position(1, target_x, target_y, target_z);
            delay_ms(speed);
        }

        // 抬起右前腿并向左前方移动
        for (int step = 0; step < 10; step++) {
            float progress = (float)step / 10;
            float target_x = initial_foot_x[5] - progress * step_length;
            float target_y = initial_foot_y[5] - progress * 20.0f;
            float target_z = base_height + progress * step_height;
            leg_move_to_global_position(6, target_x, target_y, target_z);
            delay_ms(speed);
        }

        // 放下右前腿
        for (int step = 0; step < 10; step++) {
            float progress = (float)step / 10;
            float target_x = initial_foot_x[5] - step_length + progress * step_length;
            float target_y = initial_foot_y[5] - 20.0f + progress * 20.0f;
            float target_z = base_height + step_height - progress * step_height;
            leg_move_to_global_position(6, target_x, target_y, target_z);
            delay_ms(speed);
        }
    }

    // 回到初始站立位置
    //action_stand_at_height(base_height);
   // printf("交叉步动作完成\r\n");
}

/**
 * 踮脚尖动作函数 - 模拟踮脚尖站立
 * @param count: 次数
 * @param speed: 动作速度
 */
void action_tip_toe(int count, int speed) {
   // printf("执行踮脚尖动作\r\n");

    const float base_height = STAND_DEFAULT_HEIGHT;
    const float toe_height = 30.0f;

    // 记录初始脚尖位置
    float initial_foot_x[6] = {P1X, P2X, P3X, P4X, P5X, P6X};
    float initial_foot_y[6] = {P1Y, P2Y, P3Y, P4Y, P5Y, P6Y};

    // 执行踮脚尖动作
    for (int i = 0; i < count; i++) {
        // 缓慢抬起脚跟（降低z坐标）
        for (int step = 0; step < 20; step++) {
            float progress = (float)step / 20;
            float target_height = base_height - progress * toe_height;

            for (int leg = 0; leg < 6; leg++) {
                leg_move_to_global_position(leg + 1, initial_foot_x[leg], initial_foot_y[leg], target_height);
            }

            delay_ms(speed);
        }

        // 缓慢放下脚跟
        for (int step = 0; step < 20; step++) {
            float progress = (float)step / 20;
            float target_height = base_height - toe_height + progress * toe_height;

            for (int leg = 0; leg < 6; leg++) {
                leg_move_to_global_position(leg + 1, initial_foot_x[leg], initial_foot_y[leg], target_height);
            }

            delay_ms(speed);
        }
    }

    // 回到初始站立位置
    //action_stand_at_height(base_height);
    //printf("踮脚尖动作完成\r\n");
}

/**
 * 螺旋扭动动作函数 - 身体螺旋式扭动
 * @param count: 圈数
 * @param speed: 动作速度
 */
void action_spiral_twist(int count, int speed) {
    //printf("执行螺旋扭动动作\r\n");

    const float base_height = STAND_DEFAULT_HEIGHT;
    const float twist_radius = 30.0f;//旋转半径

    // 记录初始脚尖位置
    float initial_foot_x[6] = {P1X, P2X, P3X, P4X, P5X, P6X};
    float initial_foot_y[6] = {P1Y, P2Y, P3Y, P4Y, P5Y, P6Y};

    // 计算身体中心点
    float center_x = 0, center_y = 0;
    for (int i = 0; i < 6; i++) {
        center_x += initial_foot_x[i];
        center_y += initial_foot_y[i];
    }
    center_x /= 6.0f;
    center_y /= 6.0f;

    // 执行螺旋扭动动作
    for (int i = 0; i < count * 40; i++) {
        float phase = (float)i / 40 * 2 * M_PI;

        for (int leg = 0; leg < 6; leg++) {
            // 每条腿不同的相位
            float leg_phase = phase + (leg * M_PI / 3);
            float offset_x = twist_radius * cosf(leg_phase);
            float offset_y = twist_radius * sinf(leg_phase);

            float target_x = initial_foot_x[leg] + offset_x;
            float target_y = initial_foot_y[leg] + offset_y;
            leg_move_to_global_position(leg + 1, target_x, target_y, base_height);
        }

        delay_ms(speed);
    }

    // 回到初始站立位置
    //action_stand_at_height(base_height);
    //printf("螺旋扭动动作完成\r\n");
}
// ... existing code ...


/**
 * 机器人"机械舞"动作 - 模仿机械舞的僵硬动作
 * @param count: 重复次数
 * @param speed: 动作速度
 */
void action_robot_dance(int count, int speed) {
   // printf("执行机械舞动作\r\n");

    const float base_height = STAND_DEFAULT_HEIGHT;
    const float move_amplitude = 25.0f;

    // 记录初始脚尖位置
    float initial_foot_x[6] = {P1X, P2X, P3X, P4X, P5X, P6X};
    float initial_foot_y[6] = {P1Y, P2Y, P3Y, P4Y, P5Y, P6Y};

    // 执行机械舞动作
    for (int i = 0; i < count; i++) {
        // 瞬间移动到一个位置（模拟机械感）
        for (int leg = 0; leg < 6; leg++) {
            float offset_x = (leg % 2 == 0) ? move_amplitude : -move_amplitude;
            float offset_y = (leg < 3) ? move_amplitude : -move_amplitude;
            float target_z = base_height + ((leg % 3 == 0) ? move_amplitude : 0);

            leg_move_to_global_position(leg + 1,
                                      initial_foot_x[leg] + offset_x,
                                      initial_foot_y[leg] + offset_y,
                                      target_z);
        }
        delay_ms(speed * 3);

        // 回到中心位置
        for (int leg = 0; leg < 6; leg++) {
            leg_move_to_global_position(leg + 1,
                                      initial_foot_x[leg],
                                      initial_foot_y[leg],
                                      base_height);
        }
        delay_ms(speed * 2);

        // 另一种姿势
        for (int leg = 0; leg < 6; leg++) {
            float offset_x = (leg < 3) ? -move_amplitude : move_amplitude;
            float offset_y = (leg % 2 == 0) ? -move_amplitude/2 : move_amplitude/2;
            float target_z = base_height + ((leg % 2 == 0) ? -move_amplitude/2 : move_amplitude/2);

            leg_move_to_global_position(leg + 1,
                                      initial_foot_x[leg] + offset_x,
                                      initial_foot_y[leg] + offset_y,
                                      target_z);
        }
        delay_ms(speed * 3);

        // 回到中心位置
        for (int leg = 0; leg < 6; leg++) {
            leg_move_to_global_position(leg + 1,
                                      initial_foot_x[leg],
                                      initial_foot_y[leg],
                                      base_height);
        }
        delay_ms(speed * 2);
    }

    // 回到初始站立位置
    action_stand_at_height(base_height);
    //printf("机械舞动作完成\r\n");
}

/**
 * 机器人"波浪舞"动作 - 身体如波浪般起伏
 * @param count: 重复次数
 * @param speed: 动作速度
 */
void action_wave_dance(int count, int speed) {
    //printf("执行波浪舞动作\r\n");

    const float base_height = STAND_DEFAULT_HEIGHT;
    const float wave_amplitude = 30.0f;

    // 记录初始脚尖位置
    float initial_foot_x[6] = {P1X, P2X, P3X, P4X, P5X, P6X};
    float initial_foot_y[6] = {P1Y, P2Y, P3Y, P4Y, P5Y, P6Y};

    // 执行波浪舞动作
    for (int cycle = 0; cycle < count; cycle++) {
        for (int step = 0; step < 60; step++) {
            float global_phase = (float)step / 60 * 2 * M_PI;

            for (int leg = 0; leg < 6; leg++) {
                // 每条腿有不同的相位延迟，形成波浪效果
                float leg_phase = global_phase + (leg * M_PI / 3);
                float height_offset = wave_amplitude * sinf(leg_phase);

                // 同时加入一些水平方向的移动增强效果
                float x_offset = 0.5f * wave_amplitude * cosf(leg_phase);
                float y_offset = 0.3f * wave_amplitude * sinf(2 * leg_phase);

                leg_move_to_global_position(leg + 1,
                                          initial_foot_x[leg] + x_offset,
                                          initial_foot_y[leg] + y_offset,
                                          base_height + height_offset);
            }

            delay_ms(speed);
        }
    }

    // 回到初始站立位置
    //action_stand_at_height(base_height);
    //printf("波浪舞动作完成\r\n");
}

/**
 * 机器人"摇摆舞"动作 - 模仿迪斯科舞蹈
 * @param count: 重复次数
 * @param speed: 动作速度
 */
void action_disco_dance(int count, int speed) {
    //printf("执行摇摆舞动作\r\n");

    const float base_height = STAND_DEFAULT_HEIGHT;
    const float sway_amplitude = 35.0f;
    const float bounce_amplitude = 20.0f;

    // 记录初始脚尖位置
    float initial_foot_x[6] = {P1X, P2X, P3X, P4X, P5X, P6X};
    float initial_foot_y[6] = {P1Y, P2Y, P3Y, P4Y, P5Y, P6Y};

    // 计算身体中心点
    float center_x = 0, center_y = 0;
    for (int i = 0; i < 6; i++) {
        center_x += initial_foot_x[i];
        center_y += initial_foot_y[i];
    }
    center_x /= 6.0f;
    center_y /= 6.0f;

    // 执行摇摆舞动作
    for (int i = 0; i < count; i++) {
        for (int step = 0; step < 40; step++) {
            float phase = (float)step / 40 * 2 * M_PI;

            // 水平摇摆
            float sway_offset = sway_amplitude * sinf(phase);

            // 垂直弹跳
            float bounce_offset = bounce_amplitude * sinf(2 * phase);

            // 扭转效果
            float twist_angle = 15.0f * sinf(phase * 0.5f) * M_PI / 180.0f;
            float cos_twist = cosf(twist_angle);
            float sin_twist = sinf(twist_angle);

            for (int leg = 0; leg < 6; leg++) {
                // 相对于中心点的位置
                float rel_x = initial_foot_x[leg] - center_x;
                float rel_y = initial_foot_y[leg] - center_y;

                // 应用扭转
                float twisted_x = rel_x * cos_twist - rel_y * sin_twist;
                float twisted_y = rel_x * sin_twist + rel_y * cos_twist;

                // 应用摇摆和弹跳
                float target_x = center_x + twisted_x + sway_offset * (leg < 3 ? 1 : -1);
                float target_y = center_y + twisted_y;
                float target_z = base_height + bounce_offset;

                leg_move_to_global_position(leg + 1, target_x, target_y, target_z);
            }

            delay_ms(speed);
        }
    }

    // 回到初始站立位置
    action_stand_at_height(base_height);
   // printf("摇摆舞动作完成\r\n");
}

/**
 * 机器人"弹跳"动作
 * @param count: 重复次数
 * @param speed: 动作速度
 */
void action_jump(int count, int speed) {
    //printf("执行弹跳动作\r\n");

    const float base_height = STAND_DEFAULT_HEIGHT;


    // 记录初始脚尖位置
    float initial_foot_x[6] = {P1X, P2X, P3X, P4X, P5X, P6X};
    float initial_foot_y[6] = {P1Y, P2Y, P3Y, P4Y, P5Y, P6Y};

    // 计算身体中心点
    float center_x = 0, center_y = 0;
    for (int i = 0; i < 6; i++) {
        center_x += initial_foot_x[i];
        center_y += initial_foot_y[i];
    }


    // 执行霹雳舞动作
    for (int i = 0; i < count; i++) {




        // 3. 弹跳动作
        for (int bounce = 0; bounce < 3; bounce++) {
            // 下蹲
            for (int leg = 0; leg < 6; leg++) {
                leg_move_to_global_position(leg + 1,
                                          initial_foot_x[leg],
                                          initial_foot_y[leg],
                                          base_height - 20.0f);
            }
            delay_ms(speed * 2);

            // 跳起
            for (int leg = 0; leg < 6; leg++) {
                leg_move_to_global_position(leg + 1,
                                          initial_foot_x[leg],
                                          initial_foot_y[leg],
                                          base_height + 30.0f);
            }
            delay_ms(speed);

            // 落地
            for (int leg = 0; leg < 6; leg++) {
                leg_move_to_global_position(leg + 1,
                                          initial_foot_x[leg],
                                          initial_foot_y[leg],
                                          base_height);
            }
            delay_ms(speed * 2);
        }
    }

    // 回到初始站立位置
    //action_stand_at_height(base_height);
    //printf("弹跳动作完成\r\n");
}

// ... existing code ...

/**
 * 挥手动作函数 - 模拟机器人挥手打招呼
 * @param count: 挥手次数
 * @param speed: 动作速度
 */
void action_wave_hand(int count, int speed) {
    //printf("执行挥手动作\r\n");

    const float base_height = STAND_DEFAULT_HEIGHT;
    const float wave_amplitude = 40.0f;
    const float wave_height = 150.0f;

    // 记录初始脚尖位置
    float initial_foot_x[6] = {P1X, P2X, P3X, P4X, P5X, P6X};
    float initial_foot_y[6] = {P1Y, P2Y, P3Y, P4Y, P5Y, P6Y};

    // 先让机器人站立
    action_stand_at_height(base_height);
    delay_ms(500);

    // 抬起右前腿（腿1）到挥手起始位置
    for (int step = 0; step < 20; step++) {
        float progress = (float)step / 20;
        float target_x = initial_foot_x[0] + progress * 30.0f;  // 向前伸出
        float target_z = base_height + progress * 20.0f;        // 抬高一些
        leg_move_to_global_position(1, target_x, initial_foot_y[0], target_z);
        delay_ms(speed);
    }

    // 执行挥手动作
    for (int i = 0; i < count; i++) {
        // 向右挥动
        for (int step = 0; step < 15; step++) {
            float progress = (float)step / 15;
            float target_x = (initial_foot_x[0] + 30.0f) + wave_amplitude * progress;
            float target_z = (base_height + 20.0f) + wave_height * sinf(progress * M_PI);
            leg_move_to_global_position(1, target_x, initial_foot_y[0], target_z);
            delay_ms(speed);
        }

        // 向左挥动
        for (int step = 0; step < 30; step++) {
            float progress = (float)step / 30;
            float target_x = (initial_foot_x[0] + 30.0f + wave_amplitude) - 2 * wave_amplitude * progress;
            float target_z = (base_height + 20.0f) + wave_height * sinf((1.0f - progress) * M_PI);
            leg_move_to_global_position(1, target_x, initial_foot_y[0], target_z);
            delay_ms(speed);
        }

        // 回到中间
        for (int step = 0; step < 15; step++) {
            float progress = (float)step / 15;
            float target_x = (initial_foot_x[0] + 30.0f - wave_amplitude) + wave_amplitude * progress;
            float target_z = (base_height + 20.0f) + wave_height * sinf(progress * M_PI);
            leg_move_to_global_position(1, target_x, initial_foot_y[0], target_z);
            delay_ms(speed);
        }
    }

    // 放下右前腿回到初始位置
    for (int step = 0; step < 20; step++) {
        float progress = (float)step / 20;
        float target_x = (initial_foot_x[0] + 30.0f) - progress * 30.0f;
        float target_z = (base_height + 20.0f) - progress * 20.0f;
        leg_move_to_global_position(1, target_x, initial_foot_y[0], target_z);
        delay_ms(speed);
    }

    // 回到初始站立位置
    //action_stand_at_height(base_height);
   // printf("挥手动作完成\r\n");
}

// ... existing code ...

/**
 * 前进动作函数 - 机器人向前移动一段距离
 * @param distance: 移动距离(mm)
 * @param speed: 动作速度
 */
void action_move_forward(float distance, int speed) {
   // printf("执行前进动作\r\n");

    // 使用现有的移动功能实现前进
    // 向前移动相当于在X轴正方向移动
    float duration = (distance / 100.0f) * speed * 100; // 简单计算持续时间
    action_move_to_direction(1.0f, 0.0f, (uint32_t)duration);

    // 等待动作完成
    while (gait_ctrl.state != GAIT_STATE_IDLE) {
        action_move_execute();
        delay_ms(10);
    }

    //printf("前进动作完成\r\n");
}

/**
 * 后退动作函数 - 机器人向后移动一段距离
 * @param distance: 移动距离(mm)
 * @param speed: 动作速度
 */
void action_move_backward(float distance, int speed) {
    //printf("执行后退动作\r\n");

    // 使用现有的移动功能实现后退
    // 向后移动相当于在X轴负方向移动
    float duration = (distance / 100.0f) * speed * 100; // 简单计算持续时间
    action_move_to_direction(-1.0f, 0.0f, (uint32_t)duration);

    // 等待动作完成
    while (gait_ctrl.state != GAIT_STATE_IDLE) {
        action_move_execute();
        delay_ms(10);
    }

   // printf("后退动作完成\r\n");
}

/**
 * 左移动作函数 - 机器人向左移动一段距离
 * @param distance: 移动距离(mm)
 * @param speed: 动作速度
 */
void action_move_left(float distance, int speed) {
    printf("执行左移动作\r\n");

    // 使用现有的移动功能实现向左移动
    // 向左移动相当于在Y轴正方向移动
    float duration = (distance / 100.0f) * speed * 100; // 简单计算持续时间
    action_move_to_direction(0.0f, 1.0f, (uint32_t)duration);

    // 等待动作完成
    while (gait_ctrl.state != GAIT_STATE_IDLE) {
        action_move_execute();
        delay_ms(10);
    }

   // printf("左移动作完成\r\n");
}

/**
 * 右移动作函数 - 机器人向右移动一段距离
 * @param distance: 移动距离(mm)
 * @param speed: 动作速度
 */
void action_move_right(float distance, int speed) {
    //printf("执行右移动作\r\n");

    // 使用现有的移动功能实现向右移动
    // 向右移动相当于在Y轴负方向移动
    float duration = (distance / 100.0f) * speed * 100; // 简单计算持续时间
    action_move_to_direction(0.0f, -1.0f, (uint32_t)duration);

    // 等待动作完成
    while (gait_ctrl.state != GAIT_STATE_IDLE) {
        action_move_execute();
        delay_ms(10);
    }

   // printf("右移动作完成\r\n");
}

/**
 * 左转动作函数 - 机器人向左（逆时针）转动一定角度
 * @param angle: 转动角度(度)
 * @param speed: 动作速度
 */
void action_turn_left(float angle, int speed) {
    //printf("执行左转动作\r\n");

    // 使用现有的旋转功能实现左转
    // 启动逆时针旋转
    action_turn_left_start();

    // 简单计算旋转时间
    float duration = (angle / 90.0f) * speed * 50;
    uint32_t start_time = HAL_GetTick();

    // 等待指定时间
    while ((HAL_GetTick() - start_time) < (uint32_t)duration) {
        action_move_execute();
        delay_ms(10);
    }

    // 停止旋转
    action_turn_stop();

    //printf("左转动作完成\r\n");
}

/**
 * 右转动作函数 - 机器人向右（顺时针）转动一定角度
 * @param angle: 转动角度(度)
 * @param speed: 动作速度
 */
void action_turn_right(float angle, int speed) {
    //printf("执行右转动作\r\n");

    // 使用现有的旋转功能实现右转
    // 启动顺时针旋转
    action_turn_right_start();

    // 简单计算旋转时间
    float duration = (angle / 90.0f) * speed * 50;
    uint32_t start_time = HAL_GetTick();

    // 等待指定时间
    while ((HAL_GetTick() - start_time) < (uint32_t)duration) {
        action_move_execute();
        delay_ms(10);
    }

    // 停止旋转
    action_turn_stop();

    //printf("右转动作完成\r\n");
}


